#include "robotsettings.h"
#include <QDebug>
#include "robotengine.h"
#include "robotcontrol.h"

RobotSettings::RobotSettings(QString xmlfile):
    xml(xmlfile)
{
    robotEngine=NULL;
    isHomeOffsetOK=false;
}


void RobotSettings::writexml( QString nodeName, QString nodeVal )
{
    bool ok=false;
    QDomNode tmpNode = findSubNode(m_root, nodeName, ok );
    if( ok )
    {
        writeXml( tmpNode, nodeVal );
    }else{
        addNode(  m_root,  nodeName, nodeVal );
    }
}


QString RobotSettings::readxml( QString nodeName )
{
    QString tmpStr;
    bool ok=false;
    QDomNode tmpNode = findSubNode(m_root, nodeName, ok );
    if( ok )
    {
        readXml( tmpNode, tmpStr );
    }
    return tmpStr;
}

int RobotSettings::setZeroPos( QVector<int> realPos, QVector<int> attitude )
{

    int size = realPos.size();
    if( size != attitude.size() )
    {
        qDebug()<<"Error: ecat轴数跟算法轴数对不上!"<<size<<attitude.size();
        return -1;
    }

    zeroPosition=realPos;
    QVector<int> zPos=realPos;
    for(int i=0;i<size; i++ )
    {
        //zeroPosition[i] =  currPos.at(i)+zeroPosition[i];
        //如果想要标定后反馈不为零的姿态，要给ecat写一个不为零的点
        zPos[i]=zeroPosition[i]-attitude[i];// bmq +  zPos = nowPos -->  nowPos-bmq =  zPos
    }
    robotAttitude = attitude;

    qDebug()<<"setzero :==realPos/attitude/zPos====="<< realPos<<attitude<<zPos;
    setEcatHomeOffset( zPos );
    return 0;
}

void RobotSettings::saveDataToXml()
{
    if( xml.isEmpty() )
    {
        qDebug()<< "机器人配置文件名字为空!";
        return;
    }

    if( openXml(xml, QIODevice::ReadWrite) )
    {
        for(int i=0;i< zeroOrderList.size(); i++ )
        {
            writexml( QString("zeroOrder%1").arg( i ), QString("%1").arg( zeroOrderList[i] ) );
        }

        for(int i=0;i< zeroSpeedList.size(); i++ )
        {
            writexml(  QString("zeroSpeed%1").arg( i ) ,  QString("%1").arg( zeroSpeedList[i] ) );
        }

        for(int i=0;i< safePointList.size(); i++ )
        {
            writexml(  QString("safePoint%1").arg( i ) ,  QString("%1").arg( safePointList[i] ) );
        }

        for(int i=0;i< homeMethodList.size(); i++ )
        {
            writexml(  QString("homeMethod%1").arg( i ) ,  QString("%1").arg( homeMethodList[i] ) );
        }

        for(int i=0;i< homeSpeedToSwitch.size(); i++ )
        {
            writexml(  QString("homeSpeedToSwitch%1").arg( i ) ,  QString("%1").arg( homeSpeedToSwitch[i] ) );
        }

        for(int i=0;i< homeSpeedToZero.size(); i++ )
        {
            writexml(  QString("homeSpeedToSwitch%1").arg( i ) ,  QString("%1").arg( homeSpeedToZero[i] ) );
        }

        for(int i=0;i< homeAccList.size(); i++ )
        {
            writexml(  QString("homeAcc%1").arg( i ) ,  QString("%1").arg( homeAccList[i] ) );
        }

        for(int i=0;i< zeroPosition.size(); i++ )
        {
            writexml(  QString("zeroPosition%1").arg( i ) ,  QString("%1").arg( zeroPosition[i] ) );
        }

        for(int i=0;i< robotAttitude.size(); i++ )
        {
            writexml(  QString("robotAttitude%1").arg( i ) ,  QString("%1").arg( robotAttitude[i] ) );
        }

        closeXml(true);

#if MD5CHECK_DEBUG
        md5Instance->saveFile(xml, robotEngine->robotInfo.robotId);
#endif
    }

    //setEcatHomeOffset( zeroPosition );
}

void RobotSettings::readDataFromXml()
{
    if( xml.isEmpty() )
    {
        qDebug()<< "机器人配置文件名字为空!";
        return;
    }

    if( NULL==robotEngine )
    {
        return;
    }

    int axisCount = robotEngine->robotInfo.robotMotorCount + robotEngine->robotInfo.extMotorCount;

    zeroOrderList.resize(axisCount );
    zeroSpeedList.resize( axisCount );
    safePointList.resize( axisCount );
    homeMethodList.resize( axisCount );
    homeSpeedToSwitch.resize( axisCount );
    homeSpeedToZero.resize( axisCount );
    homeAccList.resize( axisCount );
    zeroPosition.resize( axisCount );
    robotAttitude.resize( axisCount );

    if( openXml(xml, QIODevice::ReadWrite) )
    {
        for(int i=0;i< zeroOrderList.size(); i++ )
        {
            zeroOrderList[i] = readxml( QString("zeroOrder%1").arg( i ) ).toInt();
        }

        for(int i=0;i< zeroSpeedList.size(); i++ )
        {
             zeroSpeedList[i]  = readxml(  QString("zeroSpeed%1").arg( i )).toDouble();
        }

        for(int i=0;i< safePointList.size(); i++ )
        {
            safePointList[i]  = readxml(  QString("safePoint%1").arg( i )).toDouble();
        }

        for(int i=0;i< homeMethodList.size(); i++ )
        {
            homeMethodList[i]  = readxml(  QString("homeMethod%1").arg( i )).toInt();
        }

        for(int i=0;i< homeSpeedToSwitch.size(); i++ )
        {
            homeSpeedToSwitch[i]  = readxml(  QString("homeSpeedToSwitch%1").arg( i )).toInt();
        }

        for(int i=0;i< homeSpeedToZero.size(); i++ )
        {
            homeSpeedToZero[i]  = readxml(  QString("homeSpeedToSwitch%1").arg( i )).toInt();
        }

        for(int i=0;i< homeAccList.size(); i++ )
        {
            homeAccList[i]  = readxml(  QString("homeAccList%1").arg( i )).toInt();
        }

        for(int i=0;i< zeroPosition.size(); i++ )
        {
            zeroPosition[i]  = readxml(  QString("zeroPosition%1").arg( i )).toInt();
        }

        for(int i=0;i< robotAttitude.size(); i++ )
        {
            robotAttitude[i]  = readxml(  QString("robotAttitude%1").arg( i )).toInt();
        }

        closeXml();
    }

    //setEcatHomeOffset( zeroPosition );

}

void RobotSettings::setEcatHomeOffset( QVector<int> pos  )
{
    if( pos.size()>0 )
    {
        if(  robCtl.setHomeOffsetRobot( robotEngine->robotInfo.robotId,  pos ) )
        {
            isHomeOffsetOK=true;
        }
        else
        {
            isHomeOffsetOK=false;
        }
        qDebug()<<"setEcatHomeOffset="<<isHomeOffsetOK;
    }
}


//void RobotSettings::resetHomeOffset( )
//{
//    if( !isHomeOffsetOK ){
//        setHomeOffset(zeroPosition);
//    }
//}



